#include "AccurateTwoWayCoupler.h"
#include "GridField.h"
#include "NavierStokesSolver.h"
#include "DecoLogger.h"
#include "Scene.h"
#include "PCGSolver.h"
#include "PGMRESSolver.h"
#include "timer.h"
#include "SimpleStrokeOptimizer.h"
#include <tbb/tbb.h>
#include <tbb/mutex.h>
using namespace tbb;

tbb::mutex TwowayCoupler::msMutex;


#define NUM_FORM_MATRIX_THREADS 1

class ParallelMatrixFormer
{
public:
	ParallelMatrixFormer(TwowayCoupler* coupler, double timeStep, double rho) : mCoupler(coupler), mTimeStep(timeStep), mRho(rho)
	{

	}
	void operator()  (const blocked_range<size_t>& r)  const
	{
		for (size_t i = r.begin(); i != r.end(); ++i)
		{
			for (int j = 0; j < mCoupler->mNumY; ++j)
			{
				for (int k = 0; k < mCoupler->mNumZ; ++k)
				{
					if (!mCoupler->mFluidGrid->inDomain[i][j][k])
						continue;
					
					FluidSparseVector ithRow(mCoupler->mNumX, mCoupler->mNumY, mCoupler->mNumZ, mCoupler->mDim);
					mCoupler->calculateDGForCell(static_cast<int>(i), j, k, mTimeStep, mRho, ithRow);
					tbb::mutex::scoped_lock lock(msMutex);
					mCoupler->mMatrix.SetRow(static_cast<int>(i), j, k, ithRow);
				}
			}
		}
	}

private:
	TwowayCoupler* mCoupler;
	double mTimeStep;
	double mRho;
	static tbb::mutex msMutex;

};

tbb::mutex ParallelMatrixFormer::msMutex;


TwowayCoupler::TwowayCoupler() : mFluidGrid(NULL), mNumX(0), mNumY(0), mNumZ(0), mDim(0), mbUpdateU(NULL), mbUpdateV(NULL), mbUpdateW(NULL), mUAccCacheIdx(NULL), mVAccCacheIdx(NULL), mWAccCacheIdx(NULL)
{
	//mSolver	= new PGMRESSolver();
	mSolver	= new PCGSolver();
}

TwowayCoupler::TwowayCoupler(GridField* grid) : mFluidGrid(NULL), mNumX(0), mNumY(0), mNumZ(0), mDim(0), mbUpdateU(NULL), mbUpdateV(NULL), mbUpdateW(NULL), mUAccCacheIdx(NULL), mVAccCacheIdx(NULL), mWAccCacheIdx(NULL)
{	
	SetFluidGrid(grid);
	//mSolver	= new PGMRESSolver();
	mSolver	= new PCGSolver();

}

TwowayCoupler::~TwowayCoupler()
{
	if (mbUpdateU)
		Free3DArray(mbUpdateU, mNumX + 1, mNumY, mNumZ);
	if (mbUpdateV)
		Free3DArray(mbUpdateV, mNumX, mNumY + 1, mNumZ);
	if (mbUpdateW)
		Free3DArray(mbUpdateW, mNumX, mNumY, mNumZ + 1);
	if (mUAccCacheIdx)
		Free3DArray(mUAccCacheIdx, mNumX + 1, mNumY, mNumZ);
	if (mVAccCacheIdx)
		Free3DArray(mVAccCacheIdx, mNumX, mNumY + 1, mNumZ);
	if (mWAccCacheIdx)
		Free3DArray(mWAccCacheIdx, mNumX, mNumY, mNumZ + 1);
	if (mSolver)
		delete mSolver;
}
void TwowayCoupler::SetFluidGrid(GridField* grid)
{
	mFluidGrid = grid;
	mNumX = grid->numXElement;
	mNumY = grid->numYElement;
	mNumZ = grid->numZElement;

	mbUpdateU = Malloc3DArray<bool>(mNumX + 1, mNumY, mNumZ);
	mbUpdateV = Malloc3DArray<bool>(mNumX, mNumY + 1, mNumZ);
	mbUpdateW = Malloc3DArray<bool>(mNumX, mNumY, mNumZ + 1);

	mUAccCacheIdx = Malloc3DArray<int>(mNumX + 1, mNumY, mNumZ);
	MemSet3DArray(mUAccCacheIdx, -1, mNumX + 1, mNumY, mNumZ);
	mVAccCacheIdx = Malloc3DArray<int>(mNumX, mNumY + 1, mNumZ);
	MemSet3DArray(mVAccCacheIdx, -1, mNumX, mNumY + 1, mNumZ);
	mWAccCacheIdx = Malloc3DArray<int>(mNumX, mNumY, mNumZ + 1);
	MemSet3DArray(mWAccCacheIdx, -1, mNumX, mNumY, mNumZ + 1);

}

void TwowayCoupler::Projection(double timeStep, double rho, bool needFormMatrix)
{
	DecoTimer timer;
	timer.startTimer();
	double time1 = timer.getCurrentTime();

	timer.resetTimer();
	if (needFormMatrix)
	{
		formMatrixAndDivergence(timeStep, rho);
	}
	else
	{
		FormDivergence(timeStep, rho);
	}
	double time2 = timer.getCurrentTime();

	timer.resetTimer();
	solveForPressure();
	double time3 = timer.getCurrentTime();

	timer.resetTimer();
	projectVelocity(timeStep, rho);
	double time4 = timer.getCurrentTime();
#ifdef _DEBUG_TIME
	(*DecoLogger::GetSingleton())<<"Preparation time "<<time1<<" s\n";
	(*DecoLogger::GetSingleton())<<"MatrixForm time "<<time2<<" s\n";
	(*DecoLogger::GetSingleton())<<"Solver time "<<time3<<" s\n";
	(*DecoLogger::GetSingleton())<<"ProjectV time "<<time4<<" s\n";
#endif
}

void TwowayCoupler::FormMatrix(double timeStep, double rho)
{
	int counter = 0;
	for (int i = 0; i < mNumX; ++i)
	{
		for (int j = 0; j < mNumY; ++j)
		{
			for (int k = 0; k < mNumZ; ++k)
			{
				if (mFluidGrid->inDomain[i][j][k])
					counter++;
			}
		}

	}
	mDim = counter;
	mMatrix.SetDimension(mFluidGrid->numXElement, mFluidGrid->numYElement, mFluidGrid->numZElement, mDim);
	parallel_for(blocked_range<size_t>(0, mNumX, mNumX / NUM_FORM_MATRIX_THREADS), ParallelMatrixFormer(this, timeStep, rho));	
	//if (mMatrix.mDim != mMatrix.mRows.size()) //debugging code
	//{
	//	(*DecoLogger::GetSingleton()) << mMatrix.mDim << "       " << static_cast<int>(mMatrix.mRows.size()) << "\n";
	//	counter = 0;
	//	for (int i = 0; i < mNumX; ++i)
	//	{
	//		for (int j = 0; j < mNumY; ++j)
	//		{
	//			for (int k = 0; k < mNumZ; ++k)
	//			{
	//				if (mFluidGrid->inDomain[i][j][k])
	//					counter++;
	//			}
	//		}

	//	}
	//	(*DecoLogger::GetSingleton()) << "Counter is " << counter << "\n";

	//}
	mCSRMatrix.SetDim(mNumX, mNumY, mNumZ);
	mCSRMatrix.ConvertFromFluidSparseMatrix(mMatrix, true);
}

void TwowayCoupler::FormDivergence(double timeStep, double rho)
{
	mCompactRHS.clear();
	for (int ithRow = 0; ithRow < mDim; ithRow++)
	{
		const Index3& idx = mMatrix.GetIndex3D(ithRow);
		double rhs = calculateDIntermediateStepForCell(idx.m_i, idx.m_j, idx.m_k, timeStep, rho);
		mCompactRHS.push_back(rhs);
	}

}

void TwowayCoupler::formMatrixAndDivergence(double timeStep, double rho)
{
	//tbb::mutex::scoped_lock lock(msMutex);
	mMatrix.SetZero();
	mCompactRHS.clear();
	double time1 = 0, time2 = 0, time3 = 0, time4;
	DecoTimer timer;
	//timer.startTimer();
	//FormMatrix(timeStep, rho);
	//
	//time1 = timer.getCurrentTime();
	//timer.resetTimer();

	//FormDivergence(timeStep, rho);
	//time2 = timer.getCurrentTime();
	int counter = 0;
	for (int i = 0; i < mNumX; ++i)
	{
		for (int j = 0; j < mNumY; ++j)
		{
			for (int k = 0; k < mNumZ; ++k)
			{
				if (mFluidGrid->inDomain[i][j][k])
					counter++;
			}
		}

	}
	mDim = counter;
	mMatrix.SetDimension(mFluidGrid->numXElement, mFluidGrid->numYElement, mFluidGrid->numZElement, mDim);

	for (INT i = 0; i < mNumX; i++)
	{
		for (INT j = 0; j < mNumY; j++)
		{
			for (INT k = 0; k < mNumZ; k++)
			{
				if (!mFluidGrid->inDomain[i][j][k])
					continue;
				
				timer.resetTimer();
				//FluidSparseVector ithRow(mNumX, mNumY, mNumZ, mDim);
				calculateDGForCell(i, j, k, timeStep, rho, mMatrix.GetRow(i, j, k));
				time1 += timer.getCurrentTime();
				timer.resetTimer();
				//mMatrix.SetRow(i, j, k, ithRow); 
				time2 += timer.getCurrentTime();
				timer.resetTimer();
				double rhs = calculateDIntermediateStepForCell(i, j, k, timeStep, rho);
				mCompactRHS.push_back(rhs);
				time3 += timer.getCurrentTime();
			
			}
		}
	}
	timer.resetTimer();
	mCSRMatrix.SetDim(mNumX, mNumY, mNumZ);
	mCSRMatrix.ConvertFromFluidSparseMatrix(mMatrix, true);
	time4 = timer.getCurrentTime();
	//if (!mMatrix.CheckSymmetry())
	//{
	//	(*DecoLogger::GetSingleton()) << "mMatrix is not symmetric!\n";
	//}
	//if (!mMatrix.CheckMMatrix())
	//{
	//	(*DecoLogger::GetSingleton()) << "mMatrix is not an M matrix!\n";

	//}
	DOUBLE norm = 0;
	for (INT i = 0; i < mDim; i++)
		norm = max(signedAbs(mCompactRHS[i]), norm);
		
	(*DecoLogger::GetSingleton()) << "norm " << norm << "\n";
#ifdef _DEBUG_TIME
	(*DecoLogger::GetSingleton()) << "Form matrix spent " << time1 << "s\n";
	(*DecoLogger::GetSingleton()) << "Set matrix row spent " << time2 << "s\n";
	(*DecoLogger::GetSingleton()) << "Form RHS spent " << time3 << "s\n";
	(*DecoLogger::GetSingleton()) << "Convert matrix spent " << time4 << "s\n";

#endif

}
void TwowayCoupler::solveForPressure()
{
	double *result = new double[mCompactRHS.size()];
	mSolver->SetMatrix(&mCSRMatrix);
	mSolver->SetRHS(&(mCompactRHS[0]));
	mSolver->Solve(result, false);
	int len = static_cast<int>(mCompactRHS.size());
	for (int it = 0; it < len; ++it)
	{
		const Index3& index = mMatrix.GetIndex3D(it);
		int i = index.m_i;;
		int j = index.m_j;
		int k = index.m_k;
		mFluidGrid->p[i][j][k] = result[it];
	}
	delete[] result;

}
void TwowayCoupler::projectVelocity(double timeStep, double rho)
{
	MemSet3DArray<bool>(mbUpdateU, 0, mNumX + 1, mNumY, mNumZ);
	MemSet3DArray<bool>(mbUpdateV, 0, mNumX, mNumY + 1, mNumZ);
	MemSet3DArray<bool>(mbUpdateW, 0, mNumX, mNumY, mNumZ + 1);
	FluidSparseVector accAlongFace(mNumX, mNumY, mNumZ, mDim);
	for (int i = 0; i < mNumX; ++i)
	{
		for (int j = 0; j < mNumY; ++j)
		{
			for (int k = 0; k < mNumZ; ++k)
			{
				if (!mFluidGrid->inDomain[i][j][k])
					continue;
				if (!mbUpdateU[i][j][k])
				{
					accAlongFace.SetZero();
					getAccAtUFace(i, j, k, rho, accAlongFace);
					int numElements = static_cast<int>(accAlongFace.mIndices.size());
					double acc = 0;
					for (int it = 0; it < numElements; ++it)
					{
						int i = accAlongFace.mIndices[it].m_i;
						int j = accAlongFace.mIndices[it].m_j;
						int k = accAlongFace.mIndices[it].m_k;
						acc += accAlongFace.mValues[it] * mFluidGrid->p[i][j][k];
						
					}
					mFluidGrid->u[i][j][k] -= timeStep * acc;
					//if (!mFluidGrid->isSolidCell(i, j, k) && !mFluidGrid->isSolidCell(i - 1, j, k))
					//	mFluidGrid->u[i][j][k] -= timeStep / rho * (mFluidGrid->p[i][j][k] - mFluidGrid->p[i - 1][j][k]) / mFluidGrid->stepSize;
					mbUpdateU[i][j][k] = true;
				}
				if (!mbUpdateU[i + 1][j][k])
				{
					accAlongFace.SetZero();
					getAccAtUFace(i + 1, j, k, rho, accAlongFace);
					int numElements = static_cast<int>(accAlongFace.mIndices.size());
					double acc = 0;
					for (int it = 0; it < numElements; ++it)
					{
						int i = accAlongFace.mIndices[it].m_i;
						int j = accAlongFace.mIndices[it].m_j;
						int k = accAlongFace.mIndices[it].m_k;
						acc += accAlongFace.mValues[it] * mFluidGrid->p[i][j][k];

					}
					mFluidGrid->u[i + 1][j][k] -= timeStep * acc;
					//if (!mFluidGrid->isSolidCell(i, j, k) && !mFluidGrid->isSolidCell(i + 1, j, k))
					//	mFluidGrid->u[i + 1][j][k] -= timeStep / rho * (mFluidGrid->p[i + 1][j][k] - mFluidGrid->p[i][j][k]) / mFluidGrid->stepSize;
					mbUpdateU[i + 1][j][k] = true;
				}

				if (!mbUpdateV[i][j][k])
				{

					accAlongFace.SetZero();
					getAccAtVFace(i, j, k, rho, accAlongFace);
					int numElements = static_cast<int>(accAlongFace.mIndices.size());
					double acc = 0;
					for (int it = 0; it < numElements; ++it)
					{
						int i = accAlongFace.mIndices[it].m_i;
						int j = accAlongFace.mIndices[it].m_j;
						int k = accAlongFace.mIndices[it].m_k;
						acc += accAlongFace.mValues[it] * mFluidGrid->p[i][j][k];

					}
					mFluidGrid->v[i][j][k] -= timeStep * acc;
					//if (!mFluidGrid->isSolidCell(i, j, k) && !mFluidGrid->isSolidCell(i, j - 1, k))
					//	mFluidGrid->v[i][j][k] -= timeStep / rho * (mFluidGrid->p[i][j][k] - mFluidGrid->p[i][j - 1][k]) / mFluidGrid->stepSize;
					mbUpdateV[i][j][k] = true;
				}
				if (!mbUpdateV[i][j + 1][k])
				{
					accAlongFace.SetZero();
					getAccAtVFace(i, j + 1, k, rho, accAlongFace);
					int numElements = static_cast<int>(accAlongFace.mIndices.size());
					double acc = 0;
					for (int it = 0; it < numElements; ++it)
					{
						int i = accAlongFace.mIndices[it].m_i;
						int j = accAlongFace.mIndices[it].m_j;
						int k = accAlongFace.mIndices[it].m_k;
						acc += accAlongFace.mValues[it] * mFluidGrid->p[i][j][k];

					}
					mFluidGrid->v[i][j + 1][k] -= timeStep * acc;
					//if (!mFluidGrid->isSolidCell(i, j, k) && !mFluidGrid->isSolidCell(i, j + 1, k))
					//	mFluidGrid->v[i][j + 1][k] -= timeStep / rho * (mFluidGrid->p[i][j + 1][k] - mFluidGrid->p[i][j][k]) / mFluidGrid->stepSize;
					mbUpdateV[i][j + 1][k] = true;
				}

				if (!mbUpdateW[i][j][k])
				{

					accAlongFace.SetZero();
					getAccAtWFace(i, j, k, rho, accAlongFace);
					int numElements = static_cast<int>(accAlongFace.mIndices.size());
					double acc = 0;
					for (int it = 0; it < numElements; ++it)
					{
						int i = accAlongFace.mIndices[it].m_i;
						int j = accAlongFace.mIndices[it].m_j;
						int k = accAlongFace.mIndices[it].m_k;
						acc += accAlongFace.mValues[it] * mFluidGrid->p[i][j][k];

					}
					mFluidGrid->w[i][j][k] -= timeStep * acc;
					//if (!mFluidGrid->isSolidCell(i, j, k) && !mFluidGrid->isSolidCell(i, j, k - 1))
					//	mFluidGrid->w[i][j][k] -= timeStep / rho * (mFluidGrid->p[i][j][k] - mFluidGrid->p[i][j][k - 1]) / mFluidGrid->stepSize;
					mbUpdateW[i][j][k] = true;
				}
				if (!mbUpdateW[i][j][k + 1])
				{
					accAlongFace.SetZero();
					getAccAtWFace(i, j, k + 1, rho, accAlongFace);
					int numElements = static_cast<int>(accAlongFace.mIndices.size());
					double acc = 0;
					for (int it = 0; it < numElements; ++it)
					{
						int i = accAlongFace.mIndices[it].m_i;
						int j = accAlongFace.mIndices[it].m_j;
						int k = accAlongFace.mIndices[it].m_k;
						acc += accAlongFace.mValues[it] * mFluidGrid->p[i][j][k];

					}
					mFluidGrid->w[i][j][k + 1] -= timeStep * acc;
					//if (!mFluidGrid->isSolidCell(i, j, k) && !mFluidGrid->isSolidCell(i, j, k + 1))
					//	mFluidGrid->w[i][j][k + 1] -= timeStep / rho * (mFluidGrid->p[i][j][k + 1] - mFluidGrid->p[i][j][k]) / mFluidGrid->stepSize;
					mbUpdateW[i][j][k + 1] = true;
				}
			}
		}
	}
	invalidAccCache();

}

void TwowayCoupler::calculateDGForCell(int i, int j, int k, double timeStep, double rho, FluidSparseVector& result)
{
	assert(mFluidGrid->isFluidCell(i, j, k));
	getAccAtUFace(i + 1, j, k, rho, result, true);
	getAccAtUFace(i, j, k, rho, result);
	getAccAtVFace(i, j + 1, k, rho, result, true);
	getAccAtVFace(i, j, k, rho, result);
	getAccAtWFace(i, j, k + 1, rho, result, true);
	getAccAtWFace(i, j, k, rho, result);
	//result *= -1;
}
double TwowayCoupler::calculateDIntermediateStepForCell(int i, int j, int k, double timeStep, double rho)
{

// 	double ret = -(mFluidGrid->u[i + 1][j][k] - mFluidGrid->u[i][j][k]
// 	+ mFluidGrid->v[i][j + 1][k] - mFluidGrid->v[i][j][k]
// 	+ mFluidGrid->w[i][j][k + 1] - mFluidGrid->w[i][j][k])
// 		/ timeStep;

	double val1 = -(mFluidGrid->uFluidVol[i + 1][j][k] * mFluidGrid->u[i + 1][j][k] - mFluidGrid->uFluidVol[i][j][k] * mFluidGrid->u[i][j][k]
	+ mFluidGrid->vFluidVol[i][j + 1][k] * mFluidGrid->v[i][j + 1][k] - mFluidGrid->vFluidVol[i][j][k] * mFluidGrid->v[i][j][k]
	+ mFluidGrid->wFluidVol[i][j][k + 1] * mFluidGrid->w[i][j][k + 1] - mFluidGrid->wFluidVol[i][j][k] * mFluidGrid->w[i][j][k])
		/ timeStep;

	double val2 = ((mFluidGrid->uFluidVol[i + 1][j][k] - mFluidGrid->cFluidVol[i][j][k]) * mFluidGrid->u[i + 1][j][k] - (mFluidGrid->uFluidVol[i][j][k] - mFluidGrid->cFluidVol[i][j][k]) * mFluidGrid->u[i][j][k]
	+ (mFluidGrid->vFluidVol[i][j + 1][k] - mFluidGrid->cFluidVol[i][j][k]) * mFluidGrid->v[i][j + 1][k] - (mFluidGrid->vFluidVol[i][j][k] - mFluidGrid->cFluidVol[i][j][k]) * mFluidGrid->v[i][j][k]
	+ (mFluidGrid->wFluidVol[i][j][k + 1] - mFluidGrid->cFluidVol[i][j][k]) * mFluidGrid->w[i][j][k + 1] - (mFluidGrid->wFluidVol[i][j][k] - mFluidGrid->cFluidVol[i][j][k]) * mFluidGrid->w[i][j][k])
		/ timeStep;

// 	if(ret - val1 - val2 > 0.001)
// 	{
// 		val1 = 1.0;
// 	}

	return val1 + val2;
}

void TwowayCoupler::invalidAccCache()
{
	mUAccCache.clear();
	mVAccCache.clear();
	mWAccCache.clear();
	MemSet3DArray(mUAccCacheIdx, -1, mNumX + 1, mNumY, mNumZ);
	MemSet3DArray(mVAccCacheIdx, -1, mNumX, mNumY + 1, mNumZ);
	MemSet3DArray(mWAccCacheIdx, -1, mNumX, mNumY, mNumZ + 1);
}

void TwowayCoupler::getAccAtUFace(int i, int j, int k, double rho, FluidSparseVector& ret, bool neg)
{
	//ret.SetZero();
	if (mUAccCacheIdx[i][j][k] != -1)
	{
		if(!neg)
			ret += mUAccCache[mUAccCacheIdx[i][j][k]];
		else
			ret -= mUAccCache[mUAccCacheIdx[i][j][k]];

		return;
	}
	double constTerm = 1.0 / ((rho * mFluidGrid->stepSize) * (!neg ? 1.0 : -1.0));
	if (mFluidGrid->isFluidCell(i, j, k) && mFluidGrid->isFluidCell(i - 1, j, k))
	{
		// 		ret.SetValue(i, j, k, 1.0 * constTerm);
		// 		ret.SetValue(i - 1, j, k, -1.0 * constTerm);

		ret.AddValue(i, j, k, mFluidGrid->uFluidVol[i][j][k] * constTerm);
		ret.AddValue(i - 1, j, k, -mFluidGrid->uFluidVol[i][j][k] * constTerm);
	}	
	else if (mFluidGrid->isFluidCell(i, j, k) && 

		mFluidGrid->isAirCell(i - 1, j, k))
	{
		// 		ret.SetValue(i, j, k, 1.0 * constTerm);
		// 		ret.SetValue(i - 1, j, k, 0);

		ret.AddValue(i, j, k, mFluidGrid->uFluidVol[i][j][k] * constTerm);
		//ret.AddValue(i - 1, j, k, 0);
	}
	else if (mFluidGrid->isAirCell(i, j, k) && mFluidGrid->isFluidCell(i - 1, j, k))
	{
		// 		ret.SetValue(i, j, k, 0);
		// 		ret.SetValue(i - 1, j, k, -1.0 * constTerm);

		//ret.AddValue(i, j, k, 0);
		ret.AddValue(i - 1, j, k, -mFluidGrid->uFluidVol[i][j][k] * constTerm);
	}
	else if (mFluidGrid->isFluidCell(i, j, k) && mFluidGrid->isSolidCell(i - 1, j, k))
	{
		DecoSolidObject* obj = mFluidGrid->pSolid[i - 1][j][k];
		if (obj)
		{
			FluidSparseVector vec(mNumX, mNumY, mNumZ, mDim);
			vec = obj->GetPressureContributionToUFaceAcc(i, j, k, mDim);
			if(!neg)
				ret += vec;
			else
				ret -= vec;

			mUAccCache.push_back(vec);
			mUAccCacheIdx[i][j][k] = static_cast<int>(mUAccCache.size()) - 1;
		}
	}
	else if (mFluidGrid->isSolidCell(i, j, k) && mFluidGrid->isFluidCell(i - 1, j, k))
	{
		DecoSolidObject* obj = mFluidGrid->pSolid[i][j][k];
		if (obj)
		{
			FluidSparseVector vec(mNumX, mNumY, mNumZ, mDim);
			vec = obj->GetPressureContributionToUFaceAcc(i, j, k, mDim);
			if(!neg)
				ret -= vec;
			else
				ret += vec;

			mUAccCache.push_back(vec);
			mUAccCacheIdx[i][j][k] = static_cast<int>(mUAccCache.size()) - 1;
		}

	}
	else
	{
		(*DecoLogger::GetSingleton()) << "Should not reach here in FluidSolidCoupler::getAccAtUFace(" << i << ", "<< j << ", " << k << ").\n";
	}
	//	(*DecoLogger::GetSingleton()) << "num of nonzero entries : " << static_cast<int>(ret.mValues.size()) << "\n";
}
void TwowayCoupler::getAccAtVFace(int i, int j, int k, double rho, FluidSparseVector& ret, bool neg)
{
	//ret.SetZero();
	if (mVAccCacheIdx[i][j][k] != -1)
	{
		if(!neg)
			ret += mVAccCache[mVAccCacheIdx[i][j][k]];
		else
			ret -= mVAccCache[mVAccCacheIdx[i][j][k]];

		return;
	}

	double constTerm = 1.0 / ((rho * mFluidGrid->stepSize) * (!neg ? 1.0 : -1.0));

	if (mFluidGrid->isFluidCell(i, j, k) && mFluidGrid->isFluidCell(i, j - 1, k))
	{
		// 		ret.SetValue(i, j, k, 1.0 * constTerm);
		// 		ret.SetValue(i, j - 1, k, -1.0 * constTerm);

		ret.AddValue(i, j, k, mFluidGrid->vFluidVol[i][j][k] * constTerm);
		ret.AddValue(i, j - 1, k, -mFluidGrid->vFluidVol[i][j][k] * constTerm);
	}	
	else if (mFluidGrid->isFluidCell(i, j, k) && mFluidGrid->isAirCell(i, j - 1, k))
	{
		// 		ret.SetValue(i, j, k, 1.0 * constTerm);
		// 		ret.SetValue(i, j - 1, k, 0);

		ret.AddValue(i, j, k, mFluidGrid->vFluidVol[i][j][k] * constTerm);
		//ret.AddValue(i, j - 1, k, 0);
	}
	else if (mFluidGrid->isAirCell(i, j, k) && mFluidGrid->isFluidCell(i, j - 1, k))
	{
		// 		ret.SetValue(i, j, k, 0);
		// 		ret.SetValue(i, j - 1, k, -1.0 * constTerm);

		//ret.AddValue(i, j, k, 0);
		ret.AddValue(i, j - 1, k, -mFluidGrid->vFluidVol[i][j][k] * constTerm);
	}
	else if (mFluidGrid->isFluidCell(i, j, k) && mFluidGrid->isSolidCell(i, j - 1, k))
	{
		DecoSolidObject* obj = mFluidGrid->pSolid[i][j - 1][k];
		if (obj)
		{
			FluidSparseVector vec(mNumX, mNumY, 

				mNumZ, mDim);
			vec = obj->GetPressureContributionToVFaceAcc(i, j, k, mDim);
			if(!neg)
				ret += vec;
			else
				ret -= vec;

			mVAccCache.push_back(vec);
			mVAccCacheIdx[i][j][k] = static_cast<int>(mVAccCache.size()) - 1;
		}
	}
	else if (mFluidGrid->isSolidCell(i, j, k) && mFluidGrid->isFluidCell(i, j - 1, k))
	{
		DecoSolidObject* obj = mFluidGrid->pSolid[i][j][k];
		if (obj)
		{
			FluidSparseVector vec(mNumX, mNumY, mNumZ, mDim);
			vec = obj->GetPressureContributionToVFaceAcc(i, j, k, mDim);
			if(!neg)
				ret -= vec;
			else
				ret += vec;

			mVAccCache.push_back(vec);
			mVAccCacheIdx[i][j][k] = static_cast<int>(mVAccCache.size()) - 1;
		}

	}
	else
	{
		(*DecoLogger::GetSingleton()) << "Should not reach here in FluidSolidCoupler::getAccAtVFace(" << i << ", "<< j << ", " << k << ").\n";
	}
	//	(*DecoLogger::GetSingleton()) << "num of nonzero entries : " << static_cast<int>(ret.mValues.size()) << "\n";
}
void TwowayCoupler::getAccAtWFace(int i, int j, int k, double rho, FluidSparseVector& ret, bool neg)
{
	//ret.SetZero();
	if (mWAccCacheIdx[i][j][k] != -1)
	{
		if(!neg)
			ret += mWAccCache[mWAccCacheIdx[i][j][k]];
		else
			ret -= mWAccCache[mWAccCacheIdx[i][j][k]];

		return;
	}

	double constTerm = 1.0 / ((rho * mFluidGrid->stepSize) * (!neg ? 1.0 : -1.0));

	if (mFluidGrid->isFluidCell(i, j, k) && mFluidGrid->isFluidCell(i, j, k - 1))
	{
		// 		ret.SetValue(i, j, k, 1.0 * constTerm);
		// 		ret.SetValue(i, j, k - 1, -1.0 * constTerm);

		ret.AddValue(i, j, k, mFluidGrid->wFluidVol[i]

		[j][k] * constTerm);
		ret.AddValue(i, j, k - 1, -mFluidGrid->wFluidVol[i][j][k] * constTerm);
	}	
	else if (mFluidGrid->isFluidCell(i, j, k) && mFluidGrid->isAirCell(i, j, k - 1))
	{
		// 		ret.SetValue(i, j, k, 1.0 * constTerm);
		// 		ret.SetValue(i, j, k - 1, 0);

		ret.AddValue(i, j, k, mFluidGrid->wFluidVol[i][j][k] * constTerm);
		//ret.AddValue(i, j, k - 1, 0);
	}
	else if (mFluidGrid->isAirCell(i, j, k) && mFluidGrid->isFluidCell(i, j, k - 1))
	{
		// 		ret.SetValue(i, j, k, 0);
		// 		ret.SetValue(i, j, k - 1, -1.0 * constTerm);

		//ret.AddValue(i, j, k, 0);
		ret.AddValue(i, j, k - 1, -mFluidGrid->wFluidVol[i][j][k] * constTerm);
	}
	else if (mFluidGrid->isFluidCell(i, j, k) && 

		mFluidGrid->isSolidCell(i, j, k - 1))
	{
		DecoSolidObject* obj = mFluidGrid->pSolid[i][j][k - 1];
		if (obj)
		{
			FluidSparseVector vec(mNumX, mNumY, 

				mNumZ, mDim);
			vec = obj->GetPressureContributionToWFaceAcc(i, j, k, mDim);
			if(!neg)
				ret += vec;
			else
				ret -= vec;

			mWAccCache.push_back(vec);
			mWAccCacheIdx[i][j][k] = static_cast<int>(mWAccCache.size()) - 1;
		}
	}
	else if (mFluidGrid->isSolidCell(i, j, k) && mFluidGrid->isFluidCell(i, j, k - 1))
	{
		DecoSolidObject* obj = mFluidGrid->pSolid[i][j][k];
		if (obj)
		{
			FluidSparseVector vec(mNumX, mNumY, 

				mNumZ, mDim);
			vec = obj->GetPressureContributionToWFaceAcc(i, j, k, mDim);
			if(!neg)
				ret -= vec;
			else
				ret += vec;

			mWAccCache.push_back(vec);
			mWAccCacheIdx[i][j][k] = static_cast<int>(mWAccCache.size()) - 1;
		}
	}
	else
	{
		(*DecoLogger::GetSingleton()) << "Should not reach here in FluidSolidCoupler::getAccAtWFace(" << i << ", "<< j << ", " << k << ").\n";
	}
	//	(*DecoLogger::GetSingleton()) << "num of nonzero entries : " << static_cast<int>(ret.mValues.size()) << "\n";
}


CSRMatrix* TwowayCoupler::GetCSRMatrix()
{
	return &mCSRMatrix;
}

FLuidSparseMatrix* TwowayCoupler::GetFluidMatrix()
{
	return &mMatrix;
}